'''
Created on Feb 25, 2010

@author: pbrook
'''

try:
    import openravepy as orp
except ImportError:
    PKG = 'simple_grasper'
    import roslib
    roslib.load_manifest(PKG)
finally:
    import openravepy as orp

import tf.transformations as tf
from simple_grasper.msg import *
from simple_grasper.srv import *
import rospy    
    
from copy import copy
import math
from time import sleep

#Flag to indicate whether we are running this in simulation only, or 
#with the intention of moving the physical robot arm
WITH_ROBOT = False

class GraspTester(object):
    '''
    classdocs
    '''


    def __init__(self, env, wam, manip_prob):
        '''
        Constructor
        '''
        self.env = env
        self.wam = wam
        self.manipprob=manip_prob

    def test_move_to_grasp(self, soln):
        #move trajectory
        command = 'MoveManipulator armvals ' + ' '.join(str(f) for f in soln)
        zeroToGraspTrajectory = self.getTrajFromCommandStr(command)
        if len(zeroToGraspTrajectory) > 0:
            #raw_input("Grasp found, <enter> to view trajectory")
            self.test_execute_trajectory(zeroToGraspTrajectory)

    def test_pour(self):
        #Pouring is defined as raising 20cm, rotating +pi/2 about the out vector, then going back
        T_hand = self.wam.GetActiveManipulator().GetEndEffectorTransform()
        T_1 = copy(T_hand)
        T_1[2, 3] += 0.2
        self.sgg.hand = None
        #self.hand.SetTransform(T_1)
        command = 'movetohandposition matrix ' + orp.matrixSerialization(T_1)
        traj = self.getTrajFromCommandStr(command)
        print "Moving up"
        #raw_input("Press enter")
        self.test_execute_trajectory(traj)

        T_2 = copy(T_1)
        T_2 = tf.concatenate_matrices(T_2, tf.rotation_matrix(math.pi / 2, (0, 0, 1)))
        #self.hand.SetTransform(T_2)
        command = 'movetohandposition matrix ' + orp.matrixSerialization(T_2)
        traj = self.getTrajFromCommandStr(command)
        print "Pouring"
        #raw_input("Press enter")
        self.test_execute_trajectory(traj)

        #self.hand.SetTransform(T_2)
        command = 'movetohandposition matrix ' + orp.matrixSerialization(T_1)
        traj = self.getTrajFromCommandStr(command)
        print "Moving back"
        #raw_input("Press enter")
        self.test_execute_trajectory(traj)

        command = 'movetohandposition matrix ' + orp.matrixSerialization(T_hand)
        traj = self.getTrajFromCommandStr(command)
        print "Moving back"
        #raw_input("Press enter")
        self.test_execute_trajectory(traj)

    def test_handing_over(self):
        #"Handing over" is defined as raising the grasp 20cm, moving to -0.2m X, and lowering 20cm
        T_hand = self.wam.GetActiveManipulator().GetEndEffectorTransform()
        T_1 = copy(T_hand)
        T_1[2, 3] += 0.2
        #self.hand = self.sgg.hand
        #self.hand.SetTransform(T_1)
        command = 'movetohandposition matrix ' + orp.matrixSerialization(T_1)
        traj = self.getTrajFromCommandStr(command)
        print "Moving up"
        #raw_input("Press enter")
        self.test_execute_trajectory(traj)
        """
        T_2 = copy(T_1)
        T_2[1,3] = -0.2
        #self.hand.SetTransform(T_2)
        #self.env.UpdatePublishedBodies()
        #raw_input("foo")
        command = 'movetohandposition matrix '+orp.matrixSerialization(T_2)
        traj = self.getTrajFromCommandStr(command)
        print "Moving over"
        raw_input("Press enter")
        self.test_execute_trajectory(traj)

        T_3 = copy(T_2)
        T_3[2,3] -= 0.2
        #self.hand.SetTransform(T_3)
        command = 'movetohandposition matrix '+orp.matrixSerialization(T_3)
        traj = self.getTrajFromCommandStr(command)
        print "Moving down"
        raw_input("Press enter")
        self.test_execute_trajectory(traj)
        """

    def getTrajFromCommandStr(self, str):
        self.manipprob.SendCommand(str)
        traj = []
        while not self.wam.GetController().IsDone():
            self.env.StepSimulation(0.01)
            self.env.UpdatePublishedBodies()
            joints = self.wam.GetActiveDOFValues()
            traj.append(JointPose7D(joints))
            sleep(0.01)
        return traj

    def test_execute_trajectory(self, traj):
        if WITH_ROBOT is False:
            return
        rospy.wait_for_service('execute_grasp_test')
        if len(traj) == 0:
            raise ValueError, "Trajectory is empty!"
        try:
            execute_grasp_test = rospy.ServiceProxy('execute_grasp_test', ExecuteGraspTest)
            self.params = TestParams(trajectory = Trajectory7D(traj), length = len(traj))
            execute_grasp_test(self.params)
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e

def test_raise():
    execute_grasp_test = rospy.ServiceProxy('execute_grasp_test', ExecuteGraspTest)
    params = TestParams(trajectory = Trajectory7D(), length = 123456789)
    execute_grasp_test(params)
